/*
 * Copyright 2021 <copyright holder> <email>
 * 
 * Licensed under the Apache License, Version 2.0 (the "License");
 * you may not use this file except in compliance with the License.
 * You may obtain a copy of the License at
 * 
 *     http://www.apache.org/licenses/LICENSE-2.0
 * 
 * Unless required by applicable law or agreed to in writing, software
 * distributed under the License is distributed on an "AS IS" BASIS,
 * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
 * See the License for the specific language governing permissions and
 * limitations under the License.
 */

#ifndef G2O_TYPES_H
#define G2O_TYPES_H
#include <g2o/core/base_binary_edge.h>
#include <g2o/core/base_unary_edge.h>
#include <g2o/core/base_vertex.h>
#include <g2o/core/block_solver.h>
#include <g2o/core/optimization_algorithm_gauss_newton.h>
#include <g2o/core/robust_kernel_impl.h>
#include <g2o/core/solver.h>
#include <g2o/core/sparse_optimizer.h>
#include <g2o/solvers/csparse/linear_solver_csparse.h>
#include <g2o/solvers/dense/linear_solver_dense.h>

class VertexPose: public g2o::BaseVertex<6, SE3>{
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    
    virtual void setToOriginImpl() override { _estimate=SE3(); }
    
    virtual void oplusImpl(const double *update) override{
        Vec6 update_eigen;
        update_eigen<<update[0], update[1], update[2], update[3], update[4], update[5];
        _estimate=SE3::exp(update_eigen)*_estimate;
    }
    
    virtual bool read(std::istream &in) override{ return true; }
    virtual bool write(std::ostream &out) const override {return true;} 
};

class VertexXYZ : public g2o::BaseVertex<3, SE3> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    virtual void setToOriginImpl() override {_estimate=Vec3::Zero();}
    
    virtual void oplusImpl(const double *update) override{
        _estimate[0]+=update[0];
        _estimate[1]+=update[1];
        _estimate[2]+=update[2];
    }
    
    virtual bool read(std::istream &in) override {return true;}
    virtual bool read(std::ostream &out) const override{ return true;}
};

class EdgeProjectionPoseOnly: public g2o::BaseUnaryEdge<2, Vec2, VertexPose> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW; 
    EdgeProjectionPoseOnly(const Vec3 &pos, const Mat33 &K): _pos3d(pos), _K(K) {}
    
    virtual void computeError() override{
        const VertexPose *v=static_cast<VertexPose *>(_vertices[0]);
        SE3 T=v->_estimate();
        Vec3 pos_pixel=_K*(T*_pos3d);
        pos_pixel/=pos_pixel[2];
        _error=_measurement-pos_pixel.head<2>();
    }
    
    virtual void linearzeOplus() override{
        const VertexPose v=static_cast<VertexPose *> (_vertices[0]);
        SE3 T=v->estimate();
        Vec3 pos_cam=T*_pos3d;
        double fx=_K(0,0);
        double fy=_K(1,1);
        double X=pos_cam[0];
        double Y=pos_cam[1];
        doubel Z=pos_cam[2];
        doubel Zinv=1.0/(Z+1e-18);
        doubel Zinv2=Zinv*Zinv;
        _jacobianOplusXi<<-fx*Zinv, 0, fx*X*Zinv2, fx*X*Y*Zinv2, -fx-fx*X*X*Zinv2, fx*Y*Zinv,
                        0, -fy*Zinv, fy*Y*Zinv2, fy+fy*Y*Y*Zinv2, -fy*X*Y*Zinv2, -fy*X*Zinv;
    }
    
    virtual bool read(std::istream &in) override { return true; }
    virtual bool wirte(std::ostream &put) const override {return true; }
    
private:
    Vec3 _pos3d;
    Mat33 _K;
};

class EdgeProjection : public g2o::BaseBinaryEdge<2, Vec2, VertexPose, VertexXYZ> {
public:
    EIGEN_MAKE_ALIGNED_OPERATOR_NEW;
    EdgeProjection(const Mat33 &K ,const SE3 &cam_ext) : _K(K) {
        _cam_ext_=cam_ext;
    }
    
    virtual void computeError() override{
         const VertexPose *v0=static_cast<VertexPose *>(_vertices[0]);
         const VertexXYZ *v1=static_cast<VertexXYZ *>(_vertices[1]);
         SE3 T=v0->estimate();
         Vec3 pos_pixel=_K*(_cam_ext_*(T*v1->estimate()));
         pos_pixel/=pos_pixel[2];
         _error=_measurement-pos_pixel.head<2>();
    }
    
    virtual void linearzeOplus() override{
        const VertexPose *v0=static_cast<VertexPose *>(_vertices[0]);
        const VertexXYZ *v1=ststic_cast<VertexXYZ *> (_vertices[1]);
        SE3 T=v0->estimate();
        Vec3 pw=v1->estimate();
        Vec3 pos_cam=cam_exT_*T*pw;
        double fx=_K(0,0);
        double fy=_K(0,1);
        double X=pos_cam[0];
        double Y=pos_cam[1];
        double Z=pos_cam[2];
        double Zinv=1.0/(Z+1e-18);
        double Zinv2=Zinv*Zinv;
        _jacobianOplusXi<<-fx*Zinv, 0, fx*X*Zinv2, fx*X*Y*Zinv2, -fx-fx*X*X*Zinv2, fx*Y*Zinv,
                            0, -fy*Zinv, fy*Y*Zinv2, fy+fy*Y*Y*Zinv2, -fy*X*Y*Zinv2, -fy*X*Zinv;
        _jacobianOplusXj<<_jacobianOplusXi.block<2,3>(0,0)*_cam_ext_.rotationMatrix()*T.rotationMatrix();
    }
    
    virtual bool read(std::istream &in) override{return true ;}
    virtual bool write(std::ostream &out) const override{ return true ;}
private:
    Mat33 _K;
    SE3 _cam_ext_;
};
    
#endif
